package com.hitqz.robot.biz.service.impl;

import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.hitqz.robot.api.business.enums.BrandEnum;
import com.hitqz.robot.api.common.entity.RobotEquipment;
import com.hitqz.robot.api.dispatch.model.MessageDto;
import com.hitqz.robot.api.dispatch.protocol.ProtocolEnum;
import com.hitqz.robot.biz.service.RealManArmClientService;
import com.hitqz.robot.biz.service.RobotEquipmentService;
import com.hitqz.robot.camera.EquipmentFactory;
import com.hitqz.robot.camera.IEquipmentService;
import com.hitqz.robot.camera.configure.EquipmentCmdEnum;
import com.hitqz.robot.camera.dto.*;
import com.hitqz.robot.camera.exception.EquipmentException;
import lombok.AllArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.context.event.EventListener;
import org.springframework.scheduling.annotation.Async;
import org.springframework.stereotype.Service;

import java.util.List;

@AllArgsConstructor
@Service
@Slf4j
public class RealManArmClientServiceImpl implements RealManArmClientService {

    private final RobotEquipmentService robotEquipmentService;
    private final EquipmentFactory equipmentFactory;

    @EventListener
    @Async("poolTaskExecutor")
    @Override
    public void listener(MessageDto dto) {
        if (dto.getCmd() < ProtocolEnum.ROBOT_REAL_MAN_ARM_STATUS.getCmd() || dto.getCmd() > ProtocolEnum.ROBOT_REAL_MAN_ARM_MOVE.getCmd()) {
            return;
        }
        log.info("receive realMan arm cmd {}", dto);
        if (dto.getCmd() == ProtocolEnum.ROBOT_REAL_MAN_ARM_SET_ZERO_POINT.getCmd()) {
            this.setZeroPoint();
        }
        if (dto.getCmd() == ProtocolEnum.ROBOT_REAL_MAN_ARM_TURN_ZERO_POINT.getCmd()) {
            this.turnZeroPoint();
        }
        if (dto.getCmd() == ProtocolEnum.ROBOT_REAL_MAN_ARM_RECOVERY.getCmd()) {
            this.emergencyStopRecovery();
        }
        if (dto.getCmd() == ProtocolEnum.ROBOT_REAL_MAN_ARM_MOVE.getCmd()) {
            RealManArmMoveDto moveDto = JSONObject.parseObject(dto.getData(), RealManArmMoveDto.class);
            this.move(moveDto);
        }
    }

    @Override
    public void setZeroPoint() {
        IEquipmentService service = equipmentFactory.getServiceByBrand(BrandEnum.REALMAN.getEnStr());
        RobotEquipment equipment = robotEquipmentService.getOne(Wrappers.lambdaQuery(RobotEquipment.class)
                .eq(RobotEquipment::getBrand, BrandEnum.REALMAN.getEnStr()));
        if (null == equipment) {
            throw new EquipmentException("请先在外设配置中配置瑞尔曼机械臂配置");
        }
        QueryParamsDto queryParamsDto = new QueryParamsDto();
        queryParamsDto.setIp(equipment.getIp());
        queryParamsDto.setPort(equipment.getPort());
        service.executeCmd(queryParamsDto, true, EquipmentCmdEnum.SET_ZERO_POINT, null);
    }

    @Override
    public void turnZeroPoint() {
        IEquipmentService service = equipmentFactory.getServiceByBrand(BrandEnum.REALMAN.getEnStr());
        RobotEquipment equipment = robotEquipmentService.getOne(Wrappers.lambdaQuery(RobotEquipment.class)
                .eq(RobotEquipment::getBrand, BrandEnum.REALMAN.getEnStr()));
        if (null == equipment) {
            throw new EquipmentException("请先在外设配置中配置瑞尔曼机械臂配置");
        }
        QueryParamsDto queryParamsDto = new QueryParamsDto();
        queryParamsDto.setIp(equipment.getIp());
        queryParamsDto.setPort(equipment.getPort());
        service.executeCmd(queryParamsDto, true, EquipmentCmdEnum.TURN_ZERO_POINT, null);
    }

    @Override
    public void emergencyStopRecovery() {
        IEquipmentService service = equipmentFactory.getServiceByBrand(BrandEnum.REALMAN.getEnStr());
        RobotEquipment equipment = robotEquipmentService.getOne(Wrappers.lambdaQuery(RobotEquipment.class)
                .eq(RobotEquipment::getBrand, BrandEnum.REALMAN.getEnStr()));
        if (null == equipment) {
            throw new EquipmentException("请先在外设配置中配置瑞尔曼机械臂配置");
        }
        QueryParamsDto queryParamsDto = new QueryParamsDto();
        queryParamsDto.setIp(equipment.getIp());
        queryParamsDto.setPort(equipment.getPort());
        service.executeCmd(queryParamsDto, true, EquipmentCmdEnum.SET_ARM_CONTINUE, null);
    }

    @Override
    public void move(RealManArmMoveDto moveDto) {
        IEquipmentService service = equipmentFactory.getServiceByBrand(BrandEnum.REALMAN.getEnStr());
        RobotEquipment equipment = robotEquipmentService.getOne(Wrappers.lambdaQuery(RobotEquipment.class)
                .eq(RobotEquipment::getBrand, BrandEnum.REALMAN.getEnStr()));
        if (null == equipment) {
            throw new EquipmentException("请先在外设配置中配置瑞尔曼机械臂配置");
        }

        IEquipmentCmdResult result = service.executeCmd(null, true, EquipmentCmdEnum.GET_CURRENT_ARM_STATE, null);
        RealManArmStateDto armState = (RealManArmStateDto) result;
        List<Integer> joint = armState.getJoint();
        final RealManArmAngleDto dto = getRealManArmAngleDto(moveDto, joint);

        QueryParamsDto queryParamsDto = new QueryParamsDto();
        queryParamsDto.setIp(equipment.getIp());
        queryParamsDto.setPort(equipment.getPort());
        service.executeCmd(queryParamsDto, true, EquipmentCmdEnum.MOVE_JOINTS, dto);
    }

    private static RealManArmAngleDto getRealManArmAngleDto(RealManArmMoveDto moveDto, List<Integer> joint) {
        RealManArmAngleDto dto = new RealManArmAngleDto();
        if (moveDto.getNum() == 1) {
            dto.setAngle1(joint.get(0) + moveDto.getJoint() * 1000);
        } else {
            dto.setAngle1(joint.get(0));
        }
        if (dto.getAngle1() > 172 * 1000 || dto.getAngle1() < -172 * 1000) {
            throw new EquipmentException("J1超出运动角度范围-172~+172");
        }
        if (moveDto.getNum() == 2) {
            dto.setAngle2(joint.get(1) + moveDto.getJoint() * 1000);
        } else {
            dto.setAngle2(joint.get(1));
        }
        if (dto.getAngle2() > 105 * 1000 || dto.getAngle2() < -105 * 1000) {
            throw new EquipmentException("J2超出运动角度范围-105~+105");
        }
        if (moveDto.getNum() == 3) {
            dto.setAngle3(joint.get(2) + moveDto.getJoint() * 1000);
        } else {
            dto.setAngle3(joint.get(2));
        }
        if (dto.getAngle3() > 172 * 1000 || dto.getAngle3() < -172 * 1000) {
            throw new EquipmentException("J3超出运动角度范围-172~+172");
        }
        if (moveDto.getNum() == 4) {
            dto.setAngle4(joint.get(3) + moveDto.getJoint() * 1000);
        } else {
            dto.setAngle4(joint.get(3));
        }
        if (dto.getAngle4() > 55 * 1000 || dto.getAngle4() < -165 * 1000) {
            throw new EquipmentException("J4超出运动角度范围-165~+55");
        }
        if (moveDto.getNum() == 5) {
            dto.setAngle5(joint.get(4) + moveDto.getJoint() * 1000);
        } else {
            dto.setAngle5(joint.get(4));
        }
        if (dto.getAngle5() > 172 * 1000 || dto.getAngle5() < -172 * 1000) {
            throw new EquipmentException("J5超出运动角度范围-172~+172");
        }
        if (moveDto.getNum() == 6) {
            dto.setAngle6(joint.get(5) + moveDto.getJoint() * 1000);
        } else {
            dto.setAngle6(joint.get(5));
        }
        if (dto.getAngle6() > 120 * 1000 || dto.getAngle6() < -90 * 1000) {
            throw new EquipmentException("J6超出运动角度范围-90~+120");
        }
        if (moveDto.getNum() == 7 && joint.size() > 6) {
            dto.setAngle7(joint.get(6) + moveDto.getJoint() * 1000);
        } else {
            dto.setAngle7(joint.get(6));
        }
        if (dto.getAngle7() > 172 * 1000 || dto.getAngle7() < -172 * 1000) {
            throw new EquipmentException("J7超出运动角度范围-172~+172");
        }
        dto.setRate(moveDto.getRate());
        return dto;
    }
}
